#include <iostream>
#include "orin_detect.h"
#include "common_structs.h"
#include "preprocess.cuh"
#include "my_serial.h"

using namespace cv;
using namespace std;
using namespace nvinfer1;

extern bool if_show;
extern toe::serial_port temp_left;
extern toe::serial_port temp_right;



static float data[1* 3* 640*640];
static float data2[1* 3* 640*640];

float* dstDevData;
float* dstDevData_right;
uchar* midDevData;
uchar* midDevData_right;
uchar* srcDevData_left;
uchar* srcDevData_right;


float* outputs_decode;
float* outputs_decode_right;
int* outputs_decode_nums;
int* outputs_decode_nums_right;
float* final_out;
float* final_out_right;
int last_alive_left = 999;
int last_type_left = -999;
int last_alive_right = 999;
int last_type_right = -999;


void Nv_Detector::decode_outputs(const float *prob, s_detections &objects,
                              int stride, int num_out, const int img_w,
                              const int img_h)
{
    float scale = std::min(param_.w / (img_w * 1.0), param_.h / (img_h * 1.0));
    cout << "scale" << scale << endl;
    std::vector<float> *l_anchor = nullptr;
    switch (stride)
    {
        case 4:
            l_anchor = &param_.a1;
            break;
        case 8:
            l_anchor = &param_.a2;
            break;
        case 16:
            l_anchor = &param_.a3;
            break;
        case 32:
            l_anchor = &param_.a4;
            break;
        default:
            assert(false && "Unknown layer stride!");
            break;
    }

    int out_h = param_.h / stride;
    int out_w = param_.w / stride;
    float pred_data[num_out];
    int nums = 0;
    for (int na = 0; na < 3; ++na)
    {
        for (int h_id = 0; h_id < out_h; ++h_id)
        {
            for (int w_id = 0; w_id < out_w; ++w_id)
            {
                int data_idx = (na * out_h * out_w + h_id * out_w + w_id) *
                            num_out;
                float obj_conf = sigmoid(prob[data_idx + 4]);
                if (obj_conf > param_.bbox_conf_thresh)
                {
                    // std::cout << data_idx+4 << std::endl;
                    sigmoid(prob + data_idx, pred_data, 5);
                    sigmoid(prob + data_idx + 15, pred_data + 15,
                            param_.classes + param_.colors + 2);
                    memcpy(pred_data + 5, prob + data_idx + 5,
                        sizeof(float) * 10);
                    int col_id = std::max_element(pred_data + 15 + param_.classes,
                                                pred_data + 15 + param_.classes +
                                                    param_.colors) -
                                (pred_data + 15 + param_.classes);
                    //if (col_id == param_.camp) continue;
                    int cls_id =
                        std::max_element(pred_data + 15,
                                        pred_data + 15 + param_.classes) -
                        (pred_data + 15);
                    
                    int t_size = std::max_element(pred_data + 15 + param_.classes + param_.colors,
                            pred_data + 15 + param_.classes + param_.colors + 2) -
                    (pred_data + 15 + param_.classes + param_.colors);

                    double final_conf =
                        obj_conf * pow(pred_data[15 + cls_id] *
                                        pred_data[15 + param_.classes + col_id] *
                                        pred_data[15 + param_.classes + param_.colors + t_size],
                                        1/3.);
                    if (final_conf > param_.bbox_conf_thresh)
                    {
                        nums++;
                        //cout << "c: " << final_conf << endl;
                        //std::cout << final_conf << " " << col_id << " "
                        //           << cls_id << std::endl;
                        
                        s_armor now;
                        float x = (pred_data[0] * 2.0 - 0.5 + w_id) *
                                stride;
                        float y = (pred_data[1] * 2.0 - 0.5 + h_id) *
                                stride;
                        float w =
                            pow(pred_data[2] * 2, 2) * l_anchor->at(na * 2);
                        float h =
                            pow(pred_data[3] * 2, 2) * l_anchor->at(na * 2 + 1);
                        //cout << "xx: " << pred_data[0] << " " << pred_data[1] << " " << pred_data[2] << " " << pred_data[3] << endl;

                        for (int p = 0; p < 5; ++p)
                        {
                            float px =
                                (pred_data[5 + p * 2] * l_anchor->at(na * 2) +
                                w_id * stride) /
                                scale;
                            float py = (pred_data[5 + p * 2 + 1] *
                                            l_anchor->at(na * 2 + 1) +
                                        h_id * stride) /
                                    scale;
                            px = std::max(std::min(px, (float)(img_w)), 0.f);
                            py = std::max(std::min(py, (float)(img_h)), 0.f);
                            now.pts[p] = Point2f(px, py);
                            //std::cout << px << " " << py  << endl;
                        }
                        // std::cout << std::endl;

                        float x0 = (x - w * 0.5) / scale;
                        float y0 = (y - h * 0.5) / scale;
                        float x1 = (x + w * 0.5) / scale;
                        float y1 = (y + h * 0.5) / scale;
                        
                        x0 = std::max(std::min(x0, (float)(img_w)), 0.f);
                        y0 = std::max(std::min(y0, (float)(img_h)), 0.f);
                        x1 = std::max(std::min(x1, (float)(img_w)), 0.f);
                        y1 = std::max(std::min(y1, (float)(img_h)), 0.f);
                        now.x_c = (x0+x1)/2;
                        now.y_c = (y0+y1)/2;
                        now.rect = Rect(x0, y0, x1 - x0, y1 - y0);
                        now.conf = final_conf;
                        now.color = col_id;
                        now.type = cls_id;
                        now.t_size = t_size;
                        objects.armor.push_back(now);
                        
                    }
                }
            }
        }
    }

}
static float calc_iou_cpu(float* a, float *b)
{
    // 获取矩形框a的坐标
    float x0_a = a[0];
    float y0_a = a[1];
    float x1_a = a[2];
    float y1_a = a[3];
    
    // 获取矩形框b的坐标
    float x0_b = b[0];
    float y0_b = b[1];
    float x1_b = b[2];
    float y1_b = b[3];
    
    // 计算交集面积
    float intersectionArea = max(0.0f, min(x1_a, x1_b) - max(x0_a, x0_b)) * max(0.0f, min(y1_a, y1_b) - max(y0_a, y0_b));
    
    // 计算并集面积
    float area_a = (x1_a - x0_a) * (y1_a - y0_a);
    float area_b = (x1_b - x0_b) * (y1_b - y0_b);
    float unionArea = area_a + area_b - intersectionArea;
    
    // 计算iou值
    float iou = intersectionArea / unionArea;
    
    return iou;
}
int Nv_Detector::simple_nms(float* decode, int n, vector<s_armor>& output_nms)
{
    output_nms.clear();
    // sort
    int idx[n];
    int picked[20];
    for (int i=0; i<n; i++)
        idx[i] = i;
    for (int i=0; i<n-1; i++)
        for (int j=0; j<n-i-j; j++)
            if (decode[idx[j]*18+12] < decode[idx[j+1]*18+12])
            {
                int temp = idx[j];
                idx[j] = idx[j+1];
                idx[j+1] = temp;
            }
    int picked_size = 0;
    cout << "after sort: "<< endl;
    for (int i=0;i<n;i++) cout << idx[i] << " ";
    cout << endl;
    for (int i=0; i<n; i++)
    {
        int idxi = idx[i];
        cout << "idxi " << idxi;
        if (decode[idxi*18+12] == 0) 
        {
            cout << "  problem" << endl;
            continue;
        }
        cout << endl;
        bool keep = 1;
        for (int j=0; j<picked_size; j++)
        {
            int idxj = picked[j];
            float iou = calc_iou_cpu(decode+idxi*18+14, decode+idxj*18+14);
            cout << "       idx: " << idxj << "  iou: " << iou << endl;
            if (iou > param_.nms_thresh || isnan(iou))
            {
                keep = 0;
                break;
            }
        }
        if (keep)
        {
            picked[picked_size] = idxi;
            picked_size++;
        }
    }

    
    for (int i=0;i<picked_size;i++)
    {
        int idx_out = picked[i];
        //(x0, y0, x1, y1, x2, y2, x3, y3, x4, y4, t_size, type, conf, color,x0,y0,x1,y1)
        s_armor now;
        float x0 = decode[idx_out*18];
        float y0 = decode[idx_out*18+1];
        float x1 = decode[idx_out*18+2];
        float y1 = decode[idx_out*18+3];
        float x2 = decode[idx_out*18+4];
        float y2 = decode[idx_out*18+5];
        float x3 = decode[idx_out*18+6];
        float y3 = decode[idx_out*18+7];
        float x4 = decode[idx_out*18+8];
        float y4 = decode[idx_out*18+9];
        float t_size = decode[idx_out*18+10];
        float type = decode[idx_out*18+11];
        float conf = decode[idx_out*18+12];
        float color = decode[idx_out*18+13];

        now.pts[0] = Point2f(x0, y0);
        now.pts[1] = Point2f(x1, y1);
        now.pts[2] = Point2f(x2, y2);
        now.pts[3] = Point2f(x3, y3);
        now.pts[4] = Point2f(x4, y4);
        x0 = decode[idx_out*18+14];
        y0 = decode[idx_out*18+15];
        x1 = decode[idx_out*18+16];
        y1 = decode[idx_out*18+17];
        now.x_c = (x0+x1)/2;
        now.y_c = (y0+y1)/2;
        now.conf = conf;
        now.color = color;
        now.type = type;
        now.t_size = t_size;
        output_nms.push_back(now);
    }
    //cout << "picked size: " << picked_size << endl;
    return picked_size;
}

void Nv_Detector::Nv_Init()
{
    zro.color = -1;
    zro.conf = -1;
    zro.t_size = -1;
    zro.x_c = -1;
    zro.y_c = -1;
    zro.type = -1;
    zro.z = -1;
    for (int i=0; i<10; i++) tracker[i] = zro;
    for (int i=0; i<10; i++) tracker_right[i] = zro;

    prior_[0] = {0, 1, 3, 4, 5, 2, 6, 7, -1};
    prior_[1] = {3, 1, 0, 4, 5, 2, 6, 7, -1};
    prior_[2] = {0, 1, 3, 4, 5, 2, 6, 7, -1};
    // do not use 3
    prior_[3] = {-1,-1,-1,-1,-1,-1,-1};

    // load engine_
    ifstream engine_file(param_.engine_file_path, ios::binary);
    cout << param_.engine_file_path << endl;
    assert(engine_file.is_open() && "Unable to load engine_ file.");
    engine_file.seekg(0, engine_file.end);
    int length = engine_file.tellg();
    engine_data_.resize(length);
    engine_file.seekg(0, engine_file.beg);
    engine_file.read(reinterpret_cast<char *>(engine_data_.data()), length);
    

    runtime_ = std::unique_ptr<nvinfer1::IRuntime>(nvinfer1::createInferRuntime(sample::gLogger.getTRTLogger()));
    if (!runtime_)
    {
        std::cout << "runtime_ create failed" << std::endl;
        exit(-1);
    }

    auto plan = engine_data_;
    
    engine_ = std::shared_ptr<nvinfer1::ICudaEngine>(runtime_->deserializeCudaEngine(plan.data(), plan.size()));
    if (!engine_)
    {
        exit(-1);
    }

    context_ = std::unique_ptr<nvinfer1::IExecutionContext>(engine_->createExecutionContext());
    if (!context_)
    {
        std::cout << "context_ create failed" << std::endl;
        exit(-1);
    }


    inputIndex = engine_->getBindingIndex("data");
    output_stride8 = engine_->getBindingIndex("stride_8");
    output_stride16 = engine_->getBindingIndex("stride_16");
    output_stride32 = engine_->getBindingIndex("stride_32");


    //preprocess 
    cudaMalloc((void**)&dstDevData, sizeof(float) * 640*640*3);
    cudaMalloc((void**)&dstDevData_right, sizeof(float) * 640*640*3);
    cudaMalloc((void**)&midDevData, sizeof(uchar) * 640*640*3);
    cudaMalloc((void**)&midDevData_right, sizeof(uchar) * 640*640*3);
    cudaMalloc((void**)&srcDevData_left, sizeof(uchar) * 640*640*3);
    cudaMalloc((void**)&srcDevData_right, sizeof(uchar) * 640*640*3);
    //resize(input_img_, input_img_, cv::Size(640,640));
    
    //cudaMemcpy(srcDevData, input_img_.data, sizeof(uchar) * 3*640*640, cudaMemcpyHostToDevice);
    //decode  
        // max_size x (x0, y0, x1, y1, x2, y2, x3, y3, x4, y4, t_size, type, conf, color,x0,y0,x1,y1)
        cudaMalloc((void**)&outputs_decode, 200*  18* sizeof(float));
        cudaMalloc((void**)&outputs_decode_right, 200*  18* sizeof(float));
        // nums 1 int
        cudaMalloc((void**)&outputs_decode_nums, sizeof(int));
        cudaMalloc((void**)&outputs_decode_nums_right, sizeof(int));
    //final out
        cudaMalloc((void**)&final_out,20*  18* sizeof(float));
        cudaMalloc((void**)&final_out_right,20*  18* sizeof(float));
    // Create GPU buffers_ on device

    CHECK(cudaMallocHost(&buffers_[inputIndex], batch_size_ * 3 * param_.h * param_.w * sizeof(float)));
    CHECK(cudaMallocHost(&buffers_[output_stride8], batch_size_ * 3*80*80*29   * sizeof(float)));
    CHECK(cudaMallocHost(&buffers_[output_stride16], batch_size_ * 3*40*40*29   * sizeof(float)));
    CHECK(cudaMallocHost(&buffers_[output_stride32], batch_size_ * 3*20*20*29   * sizeof(float)));
    
    CHECK(cudaStreamCreate(&stream_));
    

}

void Nv_Detector::preprocess()
{

    if (0)//!input_img_.empty() && !input_img2_.empty() && 0)
    {
        auto start = std::chrono::high_resolution_clock::now();
        resize(input_img_, pr_img, cv::Size(640,640));
        
        //cvtColor(input_img_, pr_img, cv::COLOR_BGR2RGB);
        
        int i = 0;
        for (int row = 0; row < param_.h; ++row) {
            uchar* uc_pixel = pr_img.data + row * pr_img.step;
            for (int col = 0; col < param_.w; ++col) {
                data[i] = (float)uc_pixel[2] / 255.0;
                data[i + param_.h * param_.w] = (float)uc_pixel[1] / 255.0;
                data[i + 2 * param_.h * param_.w] = (float)uc_pixel[0] / 255.0;
                uc_pixel += 3;
                ++i;
            }
        }

        // for (int i=0;i<3*640*640;i++) data[i] = 0;
        //for (int k=0;k<100;k++) cout<<data[k] <<endl;
        //waitKey(0);
        
        resize(input_img2_, pr_img, cv::Size(640,640));
        //cvtColor(input_img2_, pr_img, cv::COLOR_BGR2RGB);
        i = 0;
        for (int row = 0; row < param_.h; ++row) {
            uchar* uc_pixel = pr_img.data + row * pr_img.step;
            for (int col = 0; col < param_.w; ++col) {
                data2[i] = (float)uc_pixel[2] / 255.0;
                data2[i + param_.h * param_.w] = (float)uc_pixel[1] / 255.0;
                data2[i + 2 * param_.h * param_.w] = (float)uc_pixel[0] / 255.0;
                uc_pixel += 3;
                ++i;
            }
        }
        
        // for(int k=3*640*640;k<3*640*640+100;k++) cout << data[k] << endl;
        CHECK(cudaMemcpy(buffers_[inputIndex], data,3 * param_.h * param_.w*sizeof(float), cudaMemcpyHostToDevice));
        CHECK(cudaMemcpy(buffers_[inputIndex]+3 * param_.h * param_.w*sizeof(float), data2,3 * param_.h * param_.w*sizeof(float), cudaMemcpyHostToDevice));
        cudaStreamSynchronize(stream_);
        auto end = std::chrono::high_resolution_clock::now();
        float elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
        //cout << "pre: " << elapsed/1000 << endl;

        
    }

    if (1)
    {
        // resize(input_img_, input_img_, cv::Size(640,640));
        //auto start = std::chrono::high_resolution_clock::now();
        // cudaMemcpy(srcDevData, input_img_.data, 3*640*640, cudaMemcpyHostToDevice);
        // cudaDeviceSynchronize();
        //auto end = std::chrono::high_resolution_clock::now();
        //float elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
        //cout << "copy: " << elapsed/1000 << endl;

        auto start = std::chrono::high_resolution_clock::now();
        
        ppreprocess(input_img2_, buffers_[inputIndex]
            , 640, 640, dstDevData, midDevData, srcDevData_left, 640, 640);
            //, 640, 640, dstDevData, midDevData, srcDevData, 640, 640);
        cudaDeviceSynchronize();
        ppreprocess(input_img_, buffers_[inputIndex]+640*640*3*sizeof(float)
            , 640, 640, dstDevData_right, midDevData_right, srcDevData_right, 640, 640);
            //, 640, 640, dstDevData, midDevData, srcDevData, 640, 640);
        
        cudaDeviceSynchronize();
        auto end = std::chrono::high_resolution_clock::now();
        float elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
        //cout << "pre: " << elapsed/1000 << endl;
    }

    // check
    /*
    cudaMemcpy(data, buffers_[inputIndex]+640*640*3*sizeof(float), 3*640*640*sizeof(float), cudaMemcpyDeviceToHost);
    cudaDeviceSynchronize();
    cv::Mat pre_image(640, 640, CV_32FC3, data);

    cv::imwrite("pre_img.jpg", pre_image*255);
    exit(0);
    */
    

}
void Nv_Detector::inference()
{
    auto start = std::chrono::high_resolution_clock::now();
    //context_->setBindingDimensions(0, Dims4(2,3,640,640));
    context_->executeV2(buffers_);
    //context_->executeV2(buffers_);
    //context_->enqueueV2(buffers_, stream_, nullptr);
    cudaDeviceSynchronize();
    //cudaStreamSynchronize(stream_);
    //cudaDeviceSynchronize();
    auto end = std::chrono::high_resolution_clock::now();
    float elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
    //cudaStreamSynchronize(stream_);
    //cout << "infer: " << elapsed/1000 << endl;
}

void Nv_Detector::priority_filtering(int n, float* crazy, int id)
{
    vector<int> now_prior_list = prior_[now_pri_];
    // cout << "now: " << now_pri_ << endl;
    //for (int i=0; i<now_prior_list.size(); i++)
    //    cout << now_prior_list[i] << endl;
    if (id == 0)
    {
        last_alive_left++;
        if (last_alive_left > 100) 
        {
            last_alive_left = 999;
            last_type_left = -999;
        }
        if (n == 0) 
        {
            for (int i=0; i<9; i++) tracker[i] = tracker[i+1];
            tracker[9] = zro;
            return ;
        }
        
        int now_prior = 7;
        int now_index = -1;
        float min_distance = 999;
        // (x0, y0, x1, y1, x2, y2, x3, y3, x4, y4, t_size, type, conf, color(13),x0,y0,x1,y1)
        for (auto i=0; i<n; i++)
        {
            float* ptr = crazy + i*18;
            if (ptr[13] == param_.camp) continue;
            int light_add = 0;
            if (ptr[13] == 2) // gray
            {
                /*
                for (int i=0;i<10;i++)
                    if (tracker[i].type == ptr[11])
                        if (tracker[i].color == 1-param_.camp)
                            light_add++;
                */

                //cout << last_type_left << " " << last_alive_left << endl;
                if (last_type_left == ptr[11])
                    if (last_alive_left > 100) continue;
                    else;
                else
                    continue;

            }
            // good gray and enermy color
            int type = ptr[11];
            for (auto j=0; j<8; j++)
                if (now_prior_list[j] == type && now_prior > j) 
                {
                    now_prior = j;
                    now_index = i;
                }
                else if(now_prior_list[j] == type && now_prior == j)
                {
                    if (tracker[9].type == ptr[11] && tracker[9].x_c != -1)
                    {
                        float now_distance = pow(tracker[9].x_c-(ptr[14]+ptr[16])/2, 2) + pow(tracker[9].y_c-(ptr[15]+ptr[15])/2, 2);
                        if (now_distance < min_distance)
                        {
                            min_distance = now_distance;
                            now_index = i;
                        }
                    }
                    else if(ptr[12] > crazy[now_index*18+12])
                    {
                        now_index = i;
                    }
                }
        }
        //cout << "l: " << now_prior << " " << now_index << endl;
        s_armor left_out;
        left_out.x_c = -1;
        left_out.y_c = -1;
        left_out.type = -1;
        left_out.color = -1; 
        left_out.z = -1;
        if (now_prior != 7)
        {
            /***** get z   ......  **/
            if (crazy[now_index*18+13] != 2)
            {
                last_alive_left = 0;
                last_type_left = crazy[now_index*18+11];
            }
            //if (last_type_left == 2) exit(1);
            float left_bar = pow(pow((crazy[now_index*18]-crazy[now_index*18+2]),2)+pow((crazy[now_index*18+1]-crazy[now_index*18+3]),2), 0.5);
            float right_bar = pow(pow((crazy[now_index*18+4]-crazy[now_index*18+6]),2)+pow((crazy[now_index*18+5]-crazy[now_index*18+7]),2), 0.5);
            float z = (1000/(left_bar+right_bar))*param_.z_scale;
            left_out.x_c = (crazy[now_index*18]+crazy[now_index*18+2]+crazy[now_index*18+4]+crazy[now_index*18+6])/4;
            left_out.y_c = (crazy[now_index*18+1]+crazy[now_index*18+3]+crazy[now_index*18+5]+crazy[now_index*18+7])/4;
            left_out.type = crazy[now_index*18+11];
            left_out.color = crazy[now_index*18+13];
            /*
            if (z < 200 && fabs(crazy[now_index*18]-crazy[now_index*18+8]) > 350)
                left_out.z = 80;
            else
                left_out.z = z;
            */
           left_out.z = z;
        }
        for (int i=0; i<9; i++) tracker[i] = tracker[i+1];
        tracker[9] = left_out;
        //cout << "left_prior:" << left_out.type << endl; 
        // cout << "l: " << now_prior << now_index << endl;
        if (if_show)
        {
            circle(show_left, Point(int((crazy[now_index*18+14]+crazy[now_index*18+16])/2),int((crazy[now_index*18+15]+crazy[now_index*18+17])/2)), 2, Scalar(0,255,0), -1);
        }
    }
    else if (id == 1)
    {
        last_alive_right++;
        if (last_alive_right > 100) 
        {
            last_alive_right = 999;
            last_type_right = -999;
        }
        if (n == 0) 
        {
            for (int i=0; i<9; i++) tracker_right[i] = tracker_right[i+1];
            tracker_right[9] = zro;
            return ;
        }
        
        int now_prior = 7;
        int now_index = -1;
        // (x0, y0, x1, y1, x2, y2, x3, y3, x4, y4, t_size, type, conf, color(13),x0,y0,x1,y1)
        float min_distance;
        for (auto i=0; i<n; i++)
        {
            float* ptr = crazy + i*18;
            if (ptr[13] == param_.camp) continue;
            int light_add = 0;
            if (ptr[13] == 2) // gray
            {
                /*
                for (int i=0;i<10;i++)
                    if (tracker_right[i].type == ptr[11])
                        if (tracker_right[i].color == 1-param_.camp)
                            light_add++;
                if (0 && light_add < 3) continue;*/
                //cout << last_type_right << " " << ptr[11] << endl;
                //continue;
                //cout << ptr[11] << endl;
                if (last_type_right == ptr[11])
                    if (last_alive_right > 100) continue;
                    else ;
                else continue;
                //cout << "is not continue" << endl;
            }
            // good gray and enermy color
            int type = ptr[11];
            for (auto j=0; j<8; j++)
                if (now_prior_list[j] == type && now_prior > j) 
                {
                    now_prior = j;
                    now_index = i;
                }
                else if(now_prior_list[j] == type && now_prior == j)
                {
                    if (tracker_right[9].type == ptr[11] && tracker_right[9].x_c != -1)
                    {
                        float now_distance = pow(tracker_right[9].x_c-(ptr[14]+ptr[16])/2, 2) + pow(tracker_right[9].y_c-(ptr[15]+ptr[15])/2, 2);
                        if (now_distance < min_distance)
                        {
                            min_distance = now_distance;
                            now_index = i;
                        }
                    }
                    else if(ptr[12] > crazy[now_index*18+12])
                    {
                        now_index = i;
                    }
                }
        }
        //cout << "r: " << now_prior << " " << now_index << endl;
        s_armor right_out;
        right_out.x_c = -1;
        right_out.y_c = -1;
        right_out.type = -1;
        right_out.color = -1;
        right_out.z = -1;
        if (now_prior != 7)
        {
            if (crazy[now_index*18+13] != 2)
            {
                last_alive_right = 0;
                last_type_right = crazy[now_index*18+11];
            }
            
            /***** get z   ......  **/
            float left_bar = pow(pow((crazy[now_index*18]-crazy[now_index*18+2]),2)+pow((crazy[now_index*18+1]-crazy[now_index*18+3]),2), 0.5);
            float right_bar = pow(pow((crazy[now_index*18+4]-crazy[now_index*18+6]),2)+pow((crazy[now_index*18+5]-crazy[now_index*18+7]),2), 0.5);
            float z = (1000/(left_bar+right_bar))*param_.z_scale_right;
            right_out.x_c = (crazy[now_index*18]+crazy[now_index*18+2]+crazy[now_index*18+4]+crazy[now_index*18+6])/4;
            right_out.y_c = (crazy[now_index*18+1]+crazy[now_index*18+3]+crazy[now_index*18+5]+crazy[now_index*18+7])/4;
            right_out.type = crazy[now_index*18+11];
            right_out.color = crazy[now_index*18+13];
            /*
            if (z < 200 && fabs(crazy[now_index*18]-crazy[now_index*18+8]) > 300)
                right_out.z = 80;
            else
                right_out.z = z;
            */
            right_out.z = z;
        }
        for (int i=0; i<9; i++) tracker[i] = tracker[i+1];
        tracker_right[9] = right_out;
        //cout << "right_prior:" << right_out.type << endl; 
        //cout << "right_prior:" << now_prior << now_index << endl;
        if (if_show)
        {
            circle(show_right, Point(int((crazy[now_index*18+14]+crazy[now_index*18+16])/2),int((crazy[now_index*18+15]+crazy[now_index*18+17])/2)), 2, Scalar(0,255,0), -1);
        }
    }
    
}

void Nv_Detector::postprocess()
{
    if (1)
    {
        /*
            // remember run python
            // float stride8[batch_size_* 3*80*80*29];
            //CHECK(cudaMemcpy(stride8, buffers_[output_stride8],batch_size_* 3*80*80*29*sizeof(float), cudaMemcpyDeviceToHost));
            //std::ofstream file("data.bin", std::ios::binary);
            //file.write(reinterpret_cast<char*>(stride8), 2*3*80*80*29 * sizeof(float));
            //file.close();
            //exit(0);

            //float stride16[batch_size_* 3*40*40*29];
            //CHECK(cudaMemcpy(stride16, buffers_[output_stride16],batch_size_* 3*40*40*29*sizeof(float), cudaMemcpyDeviceToHost));
            //std::ofstream file("data.bin", std::ios::binary);
            //file.write(reinterpret_cast<char*>(stride16), 2*3*40*40*29 * sizeof(float));
            //file.close();
            //exit(0);

        
        float stride32[batch_size_* 3*20*20*29];
        CHECK(cudaMemcpy(stride32, buffers_[output_stride32],batch_size_* 3*20*20*29*sizeof(float), cudaMemcpyDeviceToHost));
        std::ofstream file("data.bin", std::ios::binary);
        file.write(reinterpret_cast<char*>(stride32), 2*3*20*20*29 * sizeof(float));
        file.close();
        exit(0);
        */
        cudaMalloc((void**)&outputs_decode, 200*  18* sizeof(float));
        cudaMalloc((void**)&outputs_decode_right, 200*  18* sizeof(float));
        cudaMalloc((void**)&final_out,20*  18* sizeof(float));
        cudaMalloc((void**)&final_out_right,20*  18* sizeof(float));
        auto start = std::chrono::high_resolution_clock::now();
        float* left_ptr = (float*)(buffers_[output_stride8]);
        float* right_ptr = (float*)(buffers_[output_stride8])+3*80*80*29;
        int nums[1];
        int nums_right[1];
        nums_right[0] = 0;
        nums[0] = 0;
        cudaMemcpy(outputs_decode_nums, nums, sizeof(int), cudaMemcpyHostToDevice);
        cudaDeviceSynchronize();
        cudaMemcpy(outputs_decode_nums_right, nums_right, sizeof(int), cudaMemcpyHostToDevice);
        cudaDeviceSynchronize();
        //cout << "start" << endl;
        decode_outputs_cu(left_ptr, outputs_decode,
                    8, 29, 640, 640, param_, outputs_decode_nums);
        decode_outputs_cu(right_ptr, outputs_decode_right,
                    8, 29, 640, 640, param_, outputs_decode_nums_right);


        //cudaMemcpy(nums, outputs_decode_nums, sizeof(int), cudaMemcpyDeviceToHost);
        //cout << *nums << endl;
        //exit(0);
        
        left_ptr = (float*)(buffers_[output_stride16]);
        right_ptr = (float*)(buffers_[output_stride16])+3*40*40*29;
        decode_outputs_cu(left_ptr, outputs_decode,
                        16, 29, 640, 640, param_ ,outputs_decode_nums);
        decode_outputs_cu(right_ptr, outputs_decode_right,
                        16, 29, 640, 640, param_ ,outputs_decode_nums_right);
        //cudaMemcpy(nums, outputs_decode_nums, sizeof(int), cudaMemcpyDeviceToHost);
        //cout << *nums << endl;
        
        left_ptr = (float*)(buffers_[output_stride32]);
        right_ptr = (float*)(buffers_[output_stride32])+3*20*20*29;
        decode_outputs_cu(left_ptr, outputs_decode,
                        32, 29, 640, 640, param_ ,outputs_decode_nums);
        decode_outputs_cu(right_ptr, outputs_decode_right,
                        32, 29, 640, 640, param_ ,outputs_decode_nums_right);
        cudaDeviceSynchronize();
        
        cudaMemcpy(nums, outputs_decode_nums, sizeof(int), cudaMemcpyDeviceToHost);
        cudaDeviceSynchronize();
        cudaMemcpy(nums_right, outputs_decode_nums_right, sizeof(int), cudaMemcpyDeviceToHost);
        cudaDeviceSynchronize();
        int ori_nums = *nums;
        int ori_nums_right = *nums_right;
        // cout << "nums: " << *nums  << " " << *nums_right << endl; 
        /*
            // check
            // float opt[3600];
            // CHECK(cudaMemcpy(opt, outputs_decode,3600*sizeof(float), cudaMemcpyDeviceToHost));
            // std::ofstream file("data.bin", std::ios::binary);
            // file.write(reinterpret_cast<char*>(opt), ori_nums*18 * sizeof(float));
            // file.close();
            // exit(0);
            //exit(0);
        */
       
        // copy to output_nums_
        
        cudaDeviceSynchronize(); 
        do_nms(outputs_decode, final_out, outputs_decode_nums,  param_, *nums);
        cudaDeviceSynchronize();
        do_nms(outputs_decode_right, final_out_right, outputs_decode_nums_right,  param_, *nums_right);
        cudaDeviceSynchronize();
        
        cudaMemcpy(nums, outputs_decode_nums, sizeof(int), cudaMemcpyDeviceToHost);
        cudaDeviceSynchronize();
        cudaMemcpy(nums_right, outputs_decode_nums_right, sizeof(int), cudaMemcpyDeviceToHost);
        //cout << "nums_nms_left: " << *nums << "  " << "nums_nms_right: " << *nums_right << endl;
        
        if(if_show)
        {
            show_left = input_img2_.clone();
            show_right = input_img_.clone();
        }
        float shit[360];
        cudaMemcpy(shit, final_out, 20*  18* sizeof(float), cudaMemcpyDeviceToHost);
        //cudaMemcpy(shit, outputs_decode, 200*  18* sizeof(float), cudaMemcpyDeviceToHost);
        cudaDeviceSynchronize();
        //cudaMemcpy(shit, outputs_decode, 200*  18* sizeof(float), cudaMemcpyDeviceToHost);
        cudaDeviceSynchronize();
        //int l = simple_nms(shit, ori_nums, output_nms_);
        //outputs_.armor = output_nms_;
        //show_results(show_left, outputs_);
        
        if(if_show)
        for (auto n=0;n<*nums;n++)
        {
            
            for (int p=0;p<5;p++)
            {
                float x_c = shit[n*18+p*2];
                float y_c = shit[n*18+p*2+1];
                // cout << x_c << " " << y_c << endl;
                circle(show_left, Point(int(x_c),int(y_c)), 2, Scalar(0,0,255), -1);
            }
            
            //float x_c = (shit[n*18+14]+shit[n*18+16])/2;
            //float y_c = (shit[n*18+15]+shit[n*18+17])/2;
            //circle(input_img_, Point(int(x_c),int(y_c)), 2, Scalar(0,0,255), -1);
            string text = to_string(int(shit[n*18+11])) + " " +to_string(int(shit[n*18+12]))+" "
                        +to_string(int(shit[n*18+13]));
            cv::putText(show_left, text, Point(int(shit[n*18+14]),int(shit[n*18+15])), 0, 1, Scalar(0,255,0),1, 8, 0);
            cv::rectangle(show_left,Point(int(shit[n*18+14]),int(shit[n*18+15])),
                Point(int(shit[n*18+16]),int(shit[n*18+17])),Scalar(0,255,0),1,1,0);
        }
        
        float shit1[360];
        cudaMemcpy(shit1, final_out_right, 20*  18* sizeof(float), cudaMemcpyDeviceToHost);
        //cudaMemcpy(shit1, outputs_decode_right, 200*  18* sizeof(float), cudaMemcpyDeviceToHost);
        if(if_show)
        for (auto n=0;n<*nums_right;n++)
        {
            
            for (int p=0;p<5;p++)
            {
                float x_c = shit1[n*18+p*2];
                float y_c = shit1[n*18+p*2+1];
                // cout << x_c << " " << y_c << endl;
                circle(show_right, Point(int(x_c),int(y_c)), 2, Scalar(0,0,255), -1);
            }
            
            //float x_c = (shit[n*18+14]+shit[n*18+16])/2;
            //float y_c = (shit[n*18+15]+shit[n*18+17])/2;
            //circle(input_img_, Point(int(x_c),int(y_c)), 2, Scalar(0,0,255), -1);

            string text = to_string(int(shit1[n*18+11])) + " " +to_string(int(shit1[n*18+12]))+" "
                        +to_string(int(shit1[n*18+13]));
            cv::putText(show_right, text, Point(int(shit1[n*18+14]),int(shit1[n*18+15])), 0, 1, Scalar(0,255,0),1, 8, 0);
            cv::rectangle(show_right,Point(int(shit1[n*18+14]),int(shit1[n*18+15])),
                Point(int(shit1[n*18+16]),int(shit1[n*18+17])),Scalar(0,255,0),1,1,0);
        }
        
        cudaDeviceSynchronize();
        //int r = simple_nms(shit1, ori_nums_right, output_nms2_);
        //outputs2_.armor = output_nms2_;
        //show_results(show_right, outputs2_);
        //cout << l << " " << r << endl;
        
        
            // for(int i=0;i<280;i++) cout << i << ":  " <<shit[i] << endl;
        
        
        priority_filtering(*nums, shit, temp_left.left_or_right_);
        priority_filtering(*nums_right, shit1, temp_right.left_or_right_);
        
        auto end = std::chrono::high_resolution_clock::now();
        float elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
        //cout << "post: " << elapsed/1000 << endl;
    }
    if (0)
    {
        float stride8[batch_size_* 3*80*80*29];
        float stride16[batch_size_* 3*40*40*29];
        float stride32[batch_size_* 3*20*20*29];
        CHECK(cudaMemcpy(stride8, buffers_[output_stride8],batch_size_* 3*80*80*29*sizeof(float), cudaMemcpyDeviceToHost));
        CHECK(cudaMemcpy(stride16, buffers_[output_stride16],batch_size_* 3*40*40*29*sizeof(float), cudaMemcpyDeviceToHost));
        CHECK(cudaMemcpy(stride32, buffers_[output_stride32],batch_size_* 3*20*20*29*sizeof(float), cudaMemcpyDeviceToHost));

        decode_outputs(stride8, outputs_, 8, 29, 640, 640);
        
        //std::ofstream file("data.bin", std::ios::binary);
        //file.write(reinterpret_cast<char*>(stride32), 2*3*20*20*29 * sizeof(float));
        //file.close();
        //exit(0);
        
        float *array2 = stride8+3*80*80*29;
        decode_outputs(array2, outputs2_, 8, 29, 640, 640);

        decode_outputs(stride16, outputs_, 16, 29, 640, 640);
        float *array3 = stride16+3*40*40*29;
        decode_outputs(array3, outputs2_, 16, 29, 640, 640);

        decode_outputs(stride32, outputs_, 32, 29, 640, 640);
        float *array4 = stride32+3*20*20*29;
        decode_outputs(array4, outputs2_, 32, 29, 640, 640);
    }

}

bool Nv_Detector::detect()
{
    auto start = std::chrono::high_resolution_clock::now();
    preprocess(); 
    inference(); 
    postprocess();
    auto end = std::chrono::high_resolution_clock::now();
    float elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
    //cout << elapsed/1000 << endl;
    return true;
}






